#include <trax.hpp>
#include "atUtil.h"
#include "Vector3.h"

namespace AtomTrace
{
	const Vector3 GetCrossNormal(const Vector3 & v1, const Vector3 & v2)
	{
		Vector3 N = v1.Cross(v2);
		N.Normalize();
		return N;
	}

    void ConvertLineIndexToZindexXY(int ival, int & ox, int & oy)
    {
        int tPix = ival;
        ox = oy = 0;
        for(int i=0; i<16; i++)
        {
            ox |= ((tPix >> (i*2  )) & 1) << i;
            oy |= ((tPix >> (i*2+1)) & 1) << i;
        }
    }
    
    void ConvertZcurveIndex(int ival, Vector3 & pos, int p)
    {
    	long int tPix = ival;
    	long int ox, oy, oz;
    	ox = oy = oz = 0;
    	for(int i=0; i<16; i++)
    	{
    	     ox |= ((tPix >> (i*3  )) & 1) << i;
    	     oy |= ((tPix >> (i*3+1)) & 1) << i;
    	     oz |= ((tPix >> (i*3+2)) & 1) << i;
    	}

    	float facf = 1.0f;
    	for(int i=0; i<p; i++)
    	{
    		facf *= 0.1f;
    	}
        /**/
    	pos[0] = static_cast<float>(ox) * facf;
    	pos[1] = static_cast<float>(oy) * facf;
    	pos[2] = static_cast<float>(oz) * facf;
        /**/
        //printf("int: [%i, %i, %i]\n", ox, oy, oz);
        //pos[0] = (float)ox * facf;
    	//pos[1] = (float)oy * facf;
    	//pos[2] = (float)oz * facf;
    }

    void GetZcurveIndex(const Vector3& pos, int & o_index, int p)
    {
        int fac = 1;
        for(int i=0; i<p; i++) fac *= 10;
        float facf = static_cast<float>(fac);
        Vector3 posl = pos * 0.1f;
        int ix = static_cast<int>(posl[0] * facf);
        int iy = static_cast<int>(posl[1] * facf);
        int iz = static_cast<int>(posl[2] * facf);
        // TODO:
        long int ti = 0;
        for (int i=0; i<16; i++)
        {
        	ti |= (ix & (1<<i)) << (i * 2); 	// x bin
        	ti |= (iy & (1<<i)) << (i * 2 + 1); // y bin
        	ti |= (iz & (1<<i)) << (i * 2 + 2); // z bin
        }
        o_index = ti;
    }

    const Vector3 ReflectionDir(const Vector3 & n, const Vector3 & v)
    {
    	if( n.LengthSquare() < 0.01f ) return v;
    	Vector3 ov;
    	Vector3 N = n.GetNormalize(); // Make sure it is already normalize

    	ov = N * 2.0f * (min(1.0f, v.Dot(N))) - v;
    	ov.Normalize();

    	return ov;
    }

    const Vector3 RefractionDir(const Vector3 & n, const Vector3 & iv, const float & ni, bool * totRef)
    {
    	//return n;
    	//return iv*-1.0f;
    	float vDotn = iv.Dot(n);
    	float n12 = ni;
    	if(vDotn > 0.0f) n12 = 1.0f / ni;
    	//else return n;
    	//if(vDotn > 0.0f) return iv * -1.0f;
    	//else return n;

    	float cost1 = min(1.0f, vDotn);
    	float sint1 = sqrt(1.0f - cost1 * cost1);
    	float sint2 = n12 * sint1;

    	if(sint2 >= 1.0f){
    		if(totRef != 0) *totRef = true;
    		return ReflectionDir(n, iv);
    	}
    	if(totRef != 0) *totRef = false;

    	float cost2 = sqrt(1.0f - sint2 * sint2);
    	if(vDotn > 0.0f) cost2 = -cost2;

    	Vector3 res;
    	res = n * (cost2) + (iv-n*cost1) * (-n12);
    	res.Normalize();

    	return res;
    }

    Vector3 RandomHemiDirection(const Vector3 &n)
    {
    	// since there is no triogometry function in trax, just do
    	// rejection sampling on hemi-cube

    	//Vector3 unitX(1.0f, 0.0f, 0.0f);
    	//Vector3 unitY(0.0f, 1.0f, 0.0f);
    	//Vector3 unitZ(0.0f, 0.0f, 1.0f);
    	Vector3 N = n.GetNormalize();
        //return N;

    	float x, y, z, x_2, y_2, z_2;

    	do
    	{
    		x = trax_rand();
    		y = trax_rand();
    		z = trax_rand();

    		x = x*2.0f - 1.0f;
    		y = y*2.0f - 1.0f;

    		x_2 = x * x;
    		y_2 = y * y;
    		z_2 = z * z;
    	}while(x_2 + y_2 + z_2>1.0f);

    	Vector3 tempV(x, y, z);
    	tempV.Normalize();

    	Vector3 X, Y;

    	X = GetOrtho(N);
    	Y = X.Cross(N);
        Y.Normalize();

    	tempV = X * tempV[0] + Y * tempV[1] + N * tempV[2];

    	return tempV;
    }

    Vector3 RandomDirection()
    {
    	float x, y, z, x_2, y_2, z_2;

    	do
    	{
    		x = trax_rand();
    		y = trax_rand();
    		z = trax_rand();

    		x = x*2.0f - 1.0f;
    		y = y*2.0f - 1.0f;
    		z = z*2.0f - 1.0f;

    		x_2 = x * x;
    		y_2 = y * y;
    		z_2 = z * z;
    	}while(x_2 + y_2 + z_2 > 1.0f);

    	Vector3 res(x, y, z);
    	res.Normalize();

    	return res;
    }

    Vector3 GetOrtho(const Vector3 &v)
    {
        Vector3 unitX(1.0f, 0.0f, 0.0f);
        Vector3 unitY(0.0f, 1.0f, 0.0f);

        Vector3 X;

        X = unitX.Cross(v);
        if(X.LengthSquare() < 0.01f)
        	X = unitY.Cross(v);

        X.Normalize();

        return X;
    }
}
